/*
 * @Author: yy.yy 729853861@qq.com
 * @Date: 2023-01-07 20:02:33
 * @LastEditors: yy.yy 729853861@qq.com
 * @LastEditTime: 2023-02-06 22:41:06
 * @FilePath: \lqr_ctr_simulition\lqr_ctr.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AEE
 */
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
using namespace std ; 
typedef Eigen::Matrix<double , 2 ,2 > Matrix2x2 ; 
typedef Eigen::Matrix<double , 2 ,1 > Matrix2x1 ; 
typedef Eigen::Matrix<double , 1 ,2> Matrix1x2 ;
typedef Eigen::Matrix<double , 1 , 1> Matrix1x1;

/*
    建立倒立摆空间状态方程
    x = [1 0 ; 0 10 ][x1 ; x2] + [0 ; -1]u
    u = [-k1 -k2][x1 ; x2]
*/

class LQR
{
public:
    /* data */
    Matrix2x2 A ; 
    Matrix2x1 B ;
    Matrix2x2 Q ; 
    Matrix1x1 R ; /*权值矩阵 Q R*/
    double Q_parmarment[2] ; /*因为是对角矩阵所以取对角的元素*/
public:
 LQR();
 ~LQR(){};
 Matrix1x2 cal_Riccati(); /*里卡提方程求解*/
 double cal_anwser() ;/*控制器输出求解*/
 
};
LQR:: LQR()
{
    A << 0.0,1.0,
        10.0,0.0 ;
    B << 0.0 , -1.0 ; 
    R << 0.1 ; 
    Q << 10 , 0.0,
        0.0 , 1;  
}
Matrix1x2 LQR::cal_Riccati()
{
    int N = 1500;//迭代终止次数
	double err = 1;//误差值
	double err_tolerance = 0.01;//误差收敛阈值
	Matrix2x2 Qf = Q;
	Matrix2x2 P = Qf;//迭代初始值
	//cout << "P初始矩阵为\n" << P << endl;
	Matrix2x2 Pn;//计算的最新P矩阵
	for (int iter_num = 0; iter_num < N; iter_num++) {
		Pn = Q + A.transpose() * P * A - A.transpose() * P * B * (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;//迭代公式
		//cout << "收敛误差为" << (Pn - P).array().abs().maxCoeff() << endl;
		//err = (Pn - P).array().abs().maxCoeff();//
		err = (Pn - P).lpNorm<Eigen::Infinity>();
		if(err < err_tolerance)//
		{
			P = Pn;
			std::cout << "迭代次数" << iter_num << endl;
            cout << err <<endl;
			break;
		}
		P = Pn;
			
	}
	
	cout << "P矩阵为\n" << P << endl;
	Matrix1x2 K = -(R + B.transpose() * P * B).inverse() * B.transpose() * P * A;//反馈率K
	return K;
}

double LQR::cal_anwser()
{
    
}
